# -*- coding: UTF-8 -*-
from threading import Thread
from time import sleep
import struct
import socket  # 客户端 发送一个数据，再接收一个数据
import ctypes
libgcc_s = ctypes.CDLL('libgcc_s.so.1')
import pyrealsense2 as rs
import json
import sys
import math
import copy
import numpy as np
import threading
import time
import matplotlib.pyplot as plt
#from scipy.interpolate import spline
import cv2
import cv2.aruco as aruco

# global variable
error_kp = 0.2
start_time = time.time()

class Image_converter:
    def __init__(self):

        self.dist=np.array(([[0.107320  ,-0.175569, 0.010295 , -0.004028 ,0.000000]]))
        self.dist=np.array(([[0  ,0., 0. , 0. ,0.000000]]))
        self.mtx=np.array([[607.222072 , 0.      ,   324.465533],
                          [  0.       ,  606.460759,  246.436670],
                          [  0.,           0.,           1.        ]])
        self.mtx=np.array([[926.465533 , 0.      ,   654.1],
                          [  0.       ,  926.436670,  353.1],
                          [  0.,           0.,           1.        ]])
       
        self.eye2hand=[[-0.08330238,-0.31939972 ,0.94395155],
                        [-0.99522918, -0.02161323, -0.0951407],
                        [ 0.05078976, -0.94737357, -0.31607549],
                   ]
        self.eye2hand_T=[[-0.08330238, -0.31939972,  0.94395155,  0.01117643],
                        [-0.99522918, -0.02161323, -0.0951407,   0.01222332],
                        [ 0.05078976, -0.94737357, -0.31607549, -0.10752432],
                        [ 0.,          0.,          0.,          1.        ]]

        pipeline = rs.pipeline()
        config = rs.config()
        # config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
        # Start pipeline
        profile = pipeline.start(config)
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        color_profile = color_frame.get_profile()
        cvsprofile = rs.video_stream_profile(color_profile)
        color_intrin = cvsprofile.get_intrinsics()
        # print(color_intrin)
        # Color Intrinsics 
        intr = color_frame.profile.as_video_stream_profile().intrinsics

        align_to = rs.stream.color
        align = rs.align(align_to)
        while True:
            prev_time = time.time()
            frames = pipeline.wait_for_frames()
            aligned_frames = align.process(frames)
            color_frame = aligned_frames.get_color_frame()
            c = np.asanyarray(color_frame.get_data())
            self.callback(c)
            fps = int(1/(time.time() - prev_time))
            if cv2.waitKey(fps) == ord('q'):
                pipeline.stop()
                break
        cv2.destroyAllWindows()

    def callback(self,data):
        cv_image = data
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
        parameters =  aruco.DetectorParameters_create()
        #使用aruco.detectMarkers()函数可以检测到marker，返回ID和标志板的4个角点坐标
        corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,aruco_dict,parameters=parameters)

        if ids is not None:
            global distance
            # print('---------------------')
            rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corners, 0.05, self.mtx, self.dist)
            # print("--------------id----------:",ids)
            # 估计每个标记的姿态并返回值rvet和tvec ---不同
            (rvec-tvec).any()
            for i in range(rvec.shape[0]):
                aruco.drawAxis(cv_image, self.mtx, self.dist, rvec[i, :, :], tvec[i, :, :], 0.03)
                aruco.drawDetectedMarkers(cv_image, corners)
            EulerAngles = self.rotationVectorToEulerAngles(rvec[0])


            id = ids[0][0]
            print("-----------id--------------", id)
            if id==0:
                distance = self.Dis(tvec)
            elif id==1:
                distance = self.Dis(tvec) + 0
            elif id == 2:
                distance = self.Dis(tvec) + 0
            else:
                distance = self.Dis(tvec) + 0
            print("distance",distance)
            # set Velocity
            current_time = time.time()
            run_time = current_time - start_time
            print("run_time",run_time)

            error = self.errorCallBack(distance)
            print("error", error)
            speed = self.setVel(error, error_kp, run_time)
            print("speed:", speed)

            try:
                Socke = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
                Socke.connect(('localhost', 80))
                IP_daar = Socke.getsockname()[0]
            finally:
                Socke.close()

            # send message : speed
            udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            # 2. 准备接收方的地址        
            dest_addr = (IP_daar, 6666)  # 注意 是元组，ip是字符串，端口是数字
            # 3. 从键盘获取数据
            lat = 1
            lon = 13.435456
            send_data = struct.pack('!if', lat, lon)
            # send_data = b'\x08\x00\x00\x00\x81@\x02\x00\x1d\x00\x00\x00\x10'

            # send_data = input("请输入要发送的数据:")

            # 4. 发送数据到指定的电脑上的指定程序中
            udp_socket.sendto(send_data, dest_addr)

            # --------------打印位置姿态----------------------------------
            cv2.putText(cv_image, "Attitude_angle:" + str(EulerAngles), (0, 120), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
                        (0, 255, 0), 2,
                        cv2.LINE_AA)
            cv2.putText(cv_image, "tvec:" + str(tvec[0][0]), (0, 160), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2,
                        cv2.LINE_AA)
            cv2.putText(cv_image, "distance:" + str(distance), (0, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2,
                        cv2.LINE_AA)
            cv2.putText(cv_image, "error:" + str(error), (0, 260), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2,
                        cv2.LINE_AA)
            cv2.putText(cv_image, "speed:" + str(speed), (0, 240), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2,
                        cv2.LINE_AA)
        else:
            ##### DRAW "NO IDS" #####
            self.kalman_reset_flag = False
            cv2.putText(cv_image, "No Ids", (0,64), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0),2,cv2.LINE_AA)

        # 显示结果框架
        cv2.imshow("frame",cv_image)
        key = cv2.waitKey(1)
        if key == 27:         # 按esc键退出
            print('esc break...')
            #cv2.destroyAllWindows()

    def rotationVectorToEulerAngles(self,rvec):
      R = np.zeros((3, 3), dtype=np.float64)
      cv2.Rodrigues(rvec, R)
      sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
      singular = sy < 1e-6
      if not singular:  # 偏航，俯仰，滚动
          x = math.atan2(R[2, 1], R[2, 2])
          y = math.atan2(-R[2, 0], sy)
          z = math.atan2(R[1, 0], R[0, 0])
      else:
          x = math.atan2(-R[1, 2], R[1, 1])
          y = math.atan2(-R[2, 0], sy)
          z = 0
      # 偏航，俯仰，滚动换成角度
      return x,y,z

    def Dis(self,tvec):
        dist_x = tvec[0][0][0]
        dist_y = tvec[0][0][1]
        dist_z = tvec[0][0][2]
        print("dist_x:", dist_x, "dist_y:", dist_y, "dist_z:", dist_z)
        dis = math.sqrt(dist_x * dist_x + dist_y * dist_y + dist_z * dist_z)
        print("dis", dis)
        return dis

    def errorCallBack(self,distance):
        error = distance
        if error > 0.5:
            error = 0.5
        elif error < 0.05:
            error = 0
        return error

    def setVel(self,error,error_kp,run_time):
        if run_time < 10:
            v = 0.01 * run_time
        else:
            v = error * error_kp
        return  v
    
    def GetHostIP(self):
        try:
            Socke = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            Socke.connect(('8.8.8.8', 80))
            IP = Socke.getsockname()[0]
        finally:
            Socke.close()
        print(IP)
        return IP

if __name__ == "__main__":
    #rate = rospy.Rate(30) 
    Image_converter()